#include "point.h"
#include <math.h>

using namespace m3d;

SVector2F*	m3d::VectorAdd			( SVector2F* out_vResult, const SVector2F* in_vA, const SVector2F* in_vB )
{
	out_vResult->x = in_vA->x + in_vB->x;
	out_vResult->y = in_vA->y + in_vB->y;
	return out_vResult;
};

SVector2F*	m3d::VectorAddInPlace	( SVector2F* out_vResult, const SVector2F* in_vB )
{
	out_vResult->x += in_vB->x;
	out_vResult->y += in_vB->y;
	return out_vResult;
};

SVector2F*	m3d::VectorAddScaled	( SVector2F* out_vResult, const SVector2F* in_vA, const SVector2F* in_vB, float in_fBScale )
{
	out_vResult->x = in_vA->x + in_vB->x*in_fBScale;
	out_vResult->y = in_vA->y + in_vB->y*in_fBScale;
	return out_vResult;
};

SVector2F*	m3d::VectorAddScaledInPlace( SVector2F* out_vResult, const SVector2F* in_vB, float in_fBScale )
{
	out_vResult->x	+= in_vB->x*in_fBScale;
	out_vResult->y	+= in_vB->y*in_fBScale;
	return out_vResult;
};

SVector2F*	m3d::VectorSubstract	( SVector2F* out_vResult, const SVector2F* in_vA, const SVector2F* in_vB )
{ 
	out_vResult->x = in_vA->x - in_vB->x;
	out_vResult->y = in_vA->y - in_vB->y;
	return out_vResult;
};


SVector2F*	m3d::VectorSubstractInPlace ( SVector2F* out_vResult, const SVector2F* in_vB )	
{
	out_vResult->x -= in_vB->x;
	out_vResult->y -= in_vB->y;
	return out_vResult;
};

SVector2F*	m3d::VectorSubstractScaled	( SVector2F* out_vResult, const SVector2F* in_vA, const SVector2F* in_vB, float in_fBScale )
{
	out_vResult->x	= in_vA->x - in_vB->x*in_fBScale;
	out_vResult->y	= in_vA->y - in_vB->y*in_fBScale;
	return out_vResult;
};

SVector2F*	m3d::VectorSubstractScaledInPlace( SVector2F* out_vResult, const SVector2F* in_vB, float in_fBScale )
{
	out_vResult->x	-= in_vB->x*in_fBScale;
	out_vResult->y	-= in_vB->y*in_fBScale;
	return out_vResult;
};

SVector2F*	m3d::VectorMultiply	( SVector2F* out_vResult, const SVector2F* in_Vector, float in_fScalar )
{ 
	out_vResult->x = in_Vector->x*in_fScalar;
	out_vResult->y = in_Vector->y*in_fScalar;
	return out_vResult;
};

SVector2F*	m3d::VectorMultiplyInPlace( SVector2F* out_vResult, float in_fScalar )
{ 
	out_vResult->x = out_vResult->x*in_fScalar;
	out_vResult->y = out_vResult->y*in_fScalar;
	return out_vResult;
};

SVector2F*	m3d::VectorDivide		( SVector2F* out_vResult, const SVector2F* in_Vector, float in_fScalar )
{ 
	if( in_fScalar != 0.0f )
	{
		out_vResult->x = in_Vector->x/in_fScalar;
		out_vResult->y = in_Vector->y/in_fScalar;
	}
	return out_vResult;
};


SVector2F*	m3d::VectorDivideInPlace( SVector2F* out_vResult, float in_fScalar )
{ 
	if( in_fScalar != 0.0f )
	{
		out_vResult->x = out_vResult->x/in_fScalar;
		out_vResult->y = out_vResult->y/in_fScalar;
	}
	return out_vResult;
};

float		m3d::VectorDot			( const SVector2F* in_vA, const SVector2F* in_vB )
{
	return in_vA->x*in_vB->x+
		in_vA->y*in_vB->y;
};

SVector2F* m3d::VectorNormalizeInPlace( SVector2F* out_vResult )
{
	float l 
		= out_vResult->x*out_vResult->x
		+ out_vResult->y*out_vResult->y; 
	if( 0 == l ) 
		return out_vResult;
	l = sqrt( l );
	out_vResult->x	= out_vResult->x / l;
	out_vResult->y	= out_vResult->y / l;
	return out_vResult;
};

SVector2F* m3d::VectorInterpolate				( SVector2F* inout_vResult, const SVector2F* in_vA, const SVector2F* in_vB, float fFactor )
{
	inout_vResult->x = in_vB->x*fFactor + in_vA->x*(1.0f-fFactor);
	inout_vResult->y = in_vB->y*fFactor + in_vA->y*(1.0f-fFactor);
	return inout_vResult;
};

SVector2F* m3d::VectorInterpolateInPlace		( SVector2F* inout_vResult, const SVector2F* in_vB, float fFactor )
{
	inout_vResult->x = in_vB->x*fFactor + inout_vResult->x*(1.0f-fFactor);
	inout_vResult->y = in_vB->y*fFactor + inout_vResult->y*(1.0f-fFactor);
	return inout_vResult;
};

SVector3F*	m3d::VectorAdd			( SVector3F* out_vResult, const SVector3F* in_vA, const SVector3F* in_vB )
{
	out_vResult->x = in_vA->x + in_vB->x;
	out_vResult->y = in_vA->y + in_vB->y;
	out_vResult->z = in_vA->z + in_vB->z;
	return out_vResult;
};
SVector3F*	m3d::VectorAddInPlace	( SVector3F* out_vResult, const SVector3F* in_vB )
{
	out_vResult->x += in_vB->x;
	out_vResult->y += in_vB->y;
	out_vResult->z += in_vB->z;
	return out_vResult;
};

SVector3F*	m3d::VectorAddScaled	( SVector3F* out_vResult, const SVector3F* in_vA, const SVector3F* in_vB, float in_fBScale )
{
	out_vResult->x = in_vA->x + in_vB->x*in_fBScale;
	out_vResult->y = in_vA->y + in_vB->y*in_fBScale;
	out_vResult->z = in_vA->z + in_vB->z*in_fBScale;
	return out_vResult;
};

SVector3F*	m3d::VectorAddScaledInPlace( SVector3F* out_vResult, const SVector3F* in_vB, float in_fBScale )
{
	out_vResult->x	+= in_vB->x*in_fBScale;
	out_vResult->y	+= in_vB->y*in_fBScale;
	out_vResult->z	+= in_vB->z*in_fBScale;
	return out_vResult;
};

SVector3F*	m3d::VectorSubstract	( SVector3F* out_vResult, const SVector3F* in_vA, const SVector3F* in_vB )
{ 
	out_vResult->x = in_vA->x - in_vB->x;
	out_vResult->y = in_vA->y - in_vB->y;
	out_vResult->z = in_vA->z - in_vB->z;
	return out_vResult;
};

SVector3F*	m3d::VectorSubstractInPlace ( SVector3F* out_vResult, const SVector3F* in_vB )	
{
	out_vResult->x -= in_vB->x;
	out_vResult->y -= in_vB->y;
	out_vResult->z -= in_vB->z;
	return out_vResult;
};

SVector3F*	m3d::VectorSubstractScaled	( SVector3F* out_vResult, const SVector3F* in_vA, const SVector3F* in_vB, float in_fBScale )
{
	out_vResult->x	= in_vA->x - in_vB->x*in_fBScale;
	out_vResult->y	= in_vA->y - in_vB->y*in_fBScale;
	out_vResult->z	= in_vA->z - in_vB->z*in_fBScale;
	return out_vResult;
};

SVector3F*	m3d::VectorSubstractScaledInPlace( SVector3F* out_vResult, const SVector3F* in_vB, float in_fBScale )
{
	out_vResult->x	-= in_vB->x*in_fBScale;
	out_vResult->y	-= in_vB->y*in_fBScale;
	out_vResult->z	-= in_vB->z*in_fBScale;
	return out_vResult;
};
SVector3F*	m3d::VectorMultiply	( SVector3F* out_vResult, const SVector3F* in_Vector, float in_fScalar )
{ 
	out_vResult->x = in_Vector->x*in_fScalar;
	out_vResult->y = in_Vector->y*in_fScalar;
	out_vResult->z = in_Vector->z*in_fScalar;
	return out_vResult;
};

SVector3F*	m3d::VectorMultiplyInPlace	( SVector3F* out_vResult, float in_fScalar )
{ 
	out_vResult->x = out_vResult->x*in_fScalar;
	out_vResult->y = out_vResult->y*in_fScalar;
	out_vResult->z = out_vResult->z*in_fScalar;
	return out_vResult;
};

SVector3F*	m3d::VectorDivide		( SVector3F* out_vResult, const SVector3F* in_Vector, float in_fScalar )
{ 
	if( in_fScalar != 0.0f )
	{
		out_vResult->x = in_Vector->x/in_fScalar;
		out_vResult->y = in_Vector->y/in_fScalar;
		out_vResult->z = in_Vector->z/in_fScalar;
	}
	return out_vResult;
};

SVector3F*	m3d::VectorDivideInPlace( SVector3F* out_vResult, float in_fScalar )
{ 
	if( in_fScalar != 0.0f )
	{
		out_vResult->x = out_vResult->x/in_fScalar;
		out_vResult->y = out_vResult->y/in_fScalar;
		out_vResult->z = out_vResult->z/in_fScalar;
	}
	return out_vResult;
};

SVector2F* m3d::VectorNormalize			( SVector2F* out_vResult, const SVector2F* in_vInput )
{
	float l 
		= in_vInput->x*in_vInput->x
		+ in_vInput->y*in_vInput->y; 
	if( 0 == l ) 
		return out_vResult;
	l = sqrt( l );
	out_vResult->x	= in_vInput->x / l;
	out_vResult->y	= in_vInput->y / l;
	return out_vResult;
};

float m3d::VectorDot			( const SVector3F* in_vA, const SVector3F* in_vB )
{
	return in_vA->x*in_vB->x+
		in_vA->y*in_vB->y+
		in_vA->z*in_vB->z;
};


SVector3F* m3d::VectorNormalize			( SVector3F* out_vResult, const SVector3F* in_vInput )
{
	float l 
		= in_vInput->x*in_vInput->x
		+ in_vInput->y*in_vInput->y
		+ in_vInput->z*in_vInput->z; 
	if( 0 == l ) 
		return out_vResult;
	l = sqrt( l );
	out_vResult->x	= in_vInput->x / l;
	out_vResult->y	= in_vInput->y / l;
	out_vResult->z	= in_vInput->z / l;
	return out_vResult;
};


SVector3F* m3d::VectorNormalizeInPlace( SVector3F* out_vResult )
{
	float l 
		= out_vResult->x*out_vResult->x
		+ out_vResult->y*out_vResult->y
		+ out_vResult->z*out_vResult->z; 
	if( 0 == l ) 
		return out_vResult;
	l = sqrt( l );
	out_vResult->x	= out_vResult->x / l;
	out_vResult->y	= out_vResult->y / l;
	out_vResult->z	= out_vResult->z / l;
	return out_vResult;
};

SVector3F*	m3d::VectorCross		( SVector3F* out_vResult, const SVector3F* in_vLeft, const SVector3F* in_vRight )
{ 
	out_vResult->x = in_vLeft->y * in_vRight->z - in_vLeft->z * in_vRight->y;
	out_vResult->y = in_vLeft->z * in_vRight->x - in_vLeft->x * in_vRight->z;
	out_vResult->z = in_vLeft->x * in_vRight->y - in_vLeft->y * in_vRight->x;
	return out_vResult; 
};

SVector3F*	m3d::VectorCrossAndNormalize		( SVector3F* out_vResult, const SVector3F* in_vLeft, const SVector3F* in_vRight )
{ 
	out_vResult->x = in_vLeft->y * in_vRight->z - in_vLeft->z * in_vRight->y;
	out_vResult->y = in_vLeft->z * in_vRight->x - in_vLeft->x * in_vRight->z;
	out_vResult->z = in_vLeft->x * in_vRight->y - in_vLeft->y * in_vRight->x;
	return VectorNormalizeInPlace( out_vResult ); 
};

SVector3F* m3d::VectorInterpolate				( SVector3F* inout_vResult, const SVector3F* in_vA, const SVector3F* in_vB, float fFactor )
{
	inout_vResult->x = in_vB->x*fFactor + in_vA->x*(1.0f-fFactor);
	inout_vResult->y = in_vB->y*fFactor + in_vA->y*(1.0f-fFactor);
	inout_vResult->z = in_vB->z*fFactor + in_vA->z*(1.0f-fFactor);
	return inout_vResult;
};

SVector3F* m3d::VectorInterpolateInPlace		( SVector3F* inout_vResult, const SVector3F* in_vB, float fFactor )
{
	inout_vResult->x = in_vB->x*fFactor + inout_vResult->x*(1.0f-fFactor);
	inout_vResult->y = in_vB->y*fFactor + inout_vResult->y*(1.0f-fFactor);
	inout_vResult->z = in_vB->z*fFactor + inout_vResult->z*(1.0f-fFactor);
	return inout_vResult;
};

//------ SVector4F
SVector4F*	m3d::VectorAdd			( SVector4F* out_vResult, const SVector4F* in_vA, const SVector4F* in_vB )	
{
	out_vResult->x = in_vA->x + in_vB->x;
	out_vResult->y = in_vA->y + in_vB->y;
	out_vResult->z = in_vA->z + in_vB->z;
	//out_vResult->w = 1.0f;
	return out_vResult;
};

SVector4F*	m3d::VectorAddInPlace			( SVector4F* out_vResult, const SVector4F* in_vB )	
{
	out_vResult->x += in_vB->x;
	out_vResult->y += in_vB->y;
	out_vResult->z += in_vB->z;
	//out_vResult->w = 1.0f;
	return out_vResult;
};

SVector4F*	m3d::VectorAddScaled	( SVector4F* out_vResult, const SVector4F* in_vA, const SVector4F* in_vB, float in_fBScale )
{
	out_vResult->x	= in_vA->x + in_vB->x*in_fBScale;
	out_vResult->y	= in_vA->y + in_vB->y*in_fBScale;
	out_vResult->z	= in_vA->z + in_vB->z*in_fBScale;
	//out_vResult->w	= 1.0f;
	return out_vResult;
};

SVector4F*	m3d::VectorAddScaledInPlace( SVector4F* out_vResult, const SVector4F* in_vB, float in_fBScale )
{
	out_vResult->x	+= in_vB->x*in_fBScale;
	out_vResult->y	+= in_vB->y*in_fBScale;
	out_vResult->z	+= in_vB->z*in_fBScale;
	//out_vResult->w	= 1.0f;
	return out_vResult;
};

SVector4F*	m3d::VectorSubstract	( SVector4F* out_vResult, const SVector4F* in_vA, const SVector4F* in_vB )
{ 
	out_vResult->x	= in_vA->x - in_vB->x;
	out_vResult->y	= in_vA->y - in_vB->y;
	out_vResult->z	= in_vA->z - in_vB->z;
	//out_vResult->w	= 1.0f;
	return out_vResult;
};


SVector4F*	m3d::VectorSubstractInPlace ( SVector4F* out_vResult, const SVector4F* in_vB )	
{
	out_vResult->x -= in_vB->x;
	out_vResult->y -= in_vB->y;
	out_vResult->z -= in_vB->z;
	//out_vResult->w = 1.0f;
	return out_vResult;
};

SVector4F*	m3d::VectorSubstractScaled	( SVector4F* out_vResult, const SVector4F* in_vA, const SVector4F* in_vB, float in_fBScale )
{
	out_vResult->x	= in_vA->x - in_vB->x*in_fBScale;
	out_vResult->y	= in_vA->y - in_vB->y*in_fBScale;
	out_vResult->z	= in_vA->z - in_vB->z*in_fBScale;
	//out_vResult->w	= 1.0f;
	return out_vResult;
};

SVector4F*	m3d::VectorSubstractScaledInPlace( SVector4F* out_vResult, const SVector4F* in_vB, float in_fBScale )
{
	out_vResult->x	-= in_vB->x*in_fBScale;
	out_vResult->y	-= in_vB->y*in_fBScale;
	out_vResult->z	-= in_vB->z*in_fBScale;
	//out_vResult->w	= 1.0f;
	return out_vResult;
};

SVector4F*	m3d::VectorMultiply	( SVector4F* out_vResult, const SVector4F* in_Vector, float in_fScalar )
{ 
	out_vResult->x = in_Vector->x*in_fScalar;
	out_vResult->y = in_Vector->y*in_fScalar;
	out_vResult->z = in_Vector->z*in_fScalar;
	out_vResult->w = in_Vector->w;
	return out_vResult;
};

SVector4F*	m3d::VectorMultiplyInPlace	( SVector4F* out_vResult, float in_fScalar )
{ 
	out_vResult->x = out_vResult->x*in_fScalar;
	out_vResult->y = out_vResult->y*in_fScalar;
	out_vResult->z = out_vResult->z*in_fScalar;
	//out_vResult->w = 1.0f;
	return out_vResult;
};

SVector4F*	m3d::VectorDivide		( SVector4F* out_vResult, const SVector4F* in_Vector, float in_fScalar )
{ 
	if( in_fScalar != 0.0f )
	{
		out_vResult->x = in_Vector->x/in_fScalar;
		out_vResult->y = in_Vector->y/in_fScalar;
		out_vResult->z = in_Vector->z/in_fScalar;
		out_vResult->w = in_Vector->w;
	}
	else
	{
		out_vResult->x = 0.0f;
		out_vResult->y = 0.0f;
		out_vResult->z = 0.0f;
		out_vResult->w = 1.0f;
	}

	return out_vResult;
};

SVector4F*	m3d::VectorDivideInPlace( SVector4F* out_vResult, float in_fScalar )
{ 
	if( in_fScalar != 0.0f )
	{
		out_vResult->x = out_vResult->x/in_fScalar;
		out_vResult->y = out_vResult->y/in_fScalar;
		out_vResult->z = out_vResult->z/in_fScalar;
	}
	return out_vResult;
};

float		m3d::VectorDot			( const SVector4F* in_vA, const SVector4F* in_vB )
{
	return	in_vA->x*in_vB->x+
			in_vA->y*in_vB->y+
			in_vA->z*in_vB->z;
};

SVector4F* m3d::VectorNormalize			( SVector4F* out_vResult, const SVector4F* in_vInput )
{
	float l 
		= in_vInput->x*in_vInput->x
		+ in_vInput->y*in_vInput->y
		+ in_vInput->z*in_vInput->z; 
	if( 0 == l ) 
		return out_vResult;
	l = sqrt( l );
	out_vResult->x	= in_vInput->x / l;
	out_vResult->y	= in_vInput->y / l;
	out_vResult->z	= in_vInput->z / l;
	out_vResult->w	= 1.0f; //w / l;
	return out_vResult;
};

SVector4F* m3d::VectorNormalizeInPlace( SVector4F* out_vResult )
{
	float l 
		= out_vResult->x*out_vResult->x
		+ out_vResult->y*out_vResult->y
		+ out_vResult->z*out_vResult->z; 
	if( 0 == l ) 
		return out_vResult;
	l = sqrt( l );
	out_vResult->x	= out_vResult->x / l;
	out_vResult->y	= out_vResult->y / l;
	out_vResult->z	= out_vResult->z / l;
	out_vResult->w	= 1.0f; //w / l;
	return out_vResult;
};

SVector4F* m3d::VectorInterpolate				( SVector4F* inout_vResult, const SVector4F* in_vA, const SVector4F* in_vB, float fFactor )
{
	inout_vResult->x = in_vB->x*fFactor + in_vA->x*(1.0f-fFactor);
	inout_vResult->y = in_vB->y*fFactor + in_vA->y*(1.0f-fFactor);
	inout_vResult->z = in_vB->z*fFactor + in_vA->z*(1.0f-fFactor);
	inout_vResult->w = in_vB->w*fFactor + in_vA->w*(1.0f-fFactor);
	return inout_vResult;
};

SVector4F* m3d::VectorInterpolateInPlace		( SVector4F* inout_vResult, const SVector4F* in_vB, float fFactor )
{
	inout_vResult->x = in_vB->x*fFactor + inout_vResult->x*(1.0f-fFactor);
	inout_vResult->y = in_vB->y*fFactor + inout_vResult->y*(1.0f-fFactor);
	inout_vResult->z = in_vB->z*fFactor + inout_vResult->z*(1.0f-fFactor);
	inout_vResult->w = in_vB->w*fFactor + inout_vResult->w*(1.0f-fFactor);
	return inout_vResult;
};

SQuaternion* m3d::QuaternionNormalize			( SQuaternion* out_vResult, const SQuaternion* in_vInput )
{
	float l 
		= in_vInput->w*in_vInput->w
		+ in_vInput->x*in_vInput->x
		+ in_vInput->y*in_vInput->y
		+ in_vInput->z*in_vInput->z; 
	if( 0 == l ) 
		return out_vResult;
	l = sqrt( l );
	out_vResult->x	= in_vInput->x / l;
	out_vResult->y	= in_vInput->y / l;
	out_vResult->z	= in_vInput->z / l;
	out_vResult->w	= in_vInput->w / l; //w / l;
	return out_vResult;
};

SQuaternion* m3d::QuaternionNormalizeInPlace	( SQuaternion* inout_vResult )
{
	float l 
		= inout_vResult->w*inout_vResult->w
		+ inout_vResult->x*inout_vResult->x
		+ inout_vResult->y*inout_vResult->y
		+ inout_vResult->z*inout_vResult->z; 
	if( 0 == l ) 
		return inout_vResult;
	l = sqrt( l );
	inout_vResult->x	= inout_vResult->x / l;
	inout_vResult->y	= inout_vResult->y / l;
	inout_vResult->z	= inout_vResult->z / l;
	inout_vResult->w	= inout_vResult->w / l; //w / l;
	return inout_vResult;
};

SQuaternion* m3d::QuaternionLinearInterpolate		( SQuaternion* out_vResult, const SQuaternion* in_vA, const SQuaternion* in_vB, float fFactor )
{
	//SQuaternion result = ((*in_vB)-(*in_vA))*fFactor+*in_vA;
	out_vResult->x = ( in_vB->x - in_vA->x )*fFactor+in_vA->x; //result.x;
	out_vResult->y = ( in_vB->y - in_vA->y )*fFactor+in_vA->y; //result.y;
	out_vResult->z = ( in_vB->z - in_vA->z )*fFactor+in_vA->z; //result.z;
	out_vResult->w = ( in_vB->w - in_vA->w )*fFactor+in_vA->w; //result.w;

	//out_vResult->x = ( in_vB->x*fFactor + in_vA->x*(1.0f-fFactor) ); //result.x;
	//out_vResult->y = ( in_vB->y*fFactor + in_vA->y*(1.0f-fFactor) ); //result.y;
	//out_vResult->z = ( in_vB->z*fFactor + in_vA->z*(1.0f-fFactor) ); //result.z;
	//out_vResult->w = ( in_vB->w*fFactor + in_vA->w*(1.0f-fFactor) ); //result.w;

	return out_vResult;
};

//bool m3d::operator ==( const SVector2F& a, const SVector2F& b )
//{
//	return a.x == b.x 
//		&& a.y == b.y;
//};
//
//bool m3d::operator ==( const SVector3F& a, const SVector3F& b )
//{
//	return a.x == b.x 
//		&& a.y == b.y
//		&& a.z == b.z;
//};
//
//bool m3d::operator ==( const SVector4F& a, const SVector4F& b )
//{
//	return a.x == b.x 
//		&& a.y == b.y
//		&& a.z == b.z
//		&& a.w == b.w;
//};
//
//bool m3d::operator ==( const SPlane& a, const SPlane& b )
//{
//	return a.x == b.x 
//		&& a.y == b.y
//		&& a.z == b.z
//		&& a.d == b.d;
//};
//
//bool m3d::operator !=( const SVector2F& a, const SVector2F& b )
//{
//	return a.x != b.x 
//		|| a.y != b.y;
//};
//
//bool m3d::operator !=( const SVector3F& a, const SVector3F& b )
//{
//	return a.x != b.x 
//		|| a.y != b.y
//		|| a.z != b.z;
//};
//
//bool m3d::operator !=( const SVector4F& a, const SVector4F& b )
//{
//	return a.x != b.x 
//		|| a.y != b.y
//		|| a.z != b.z
//		|| a.w != b.w;
//};
//
//bool m3d::operator !=( const SPlane& a, const SPlane& b )
//{
//	return a.x != b.x 
//		|| a.y != b.y
//		|| a.z != b.z
//		|| a.d != b.d;
//};